#ifndef ST_H
#define ST_H

#include <opencv2/opencv.hpp>
#include <iostream>
#include "pi_tracker/Skeleton.h"
#include <vector>
#include <math.h>

using namespace cv;

class Stat {
  
public:
  void invertMatrix(InputArray src, OutputArray st);
  void covarMatrix(InputArray samples, OutputArray covar, OutputArray mean);
  void stDev(std::vector< std::vector<float> > samples, float *stdev);
  std::vector<float> fitPlane(std::vector< std::vector<float> > pointsV);
  float anglePlanes(std::vector<float> plane1, std::vector<float> plane2);
  std::vector<float> fitPlanef(std::vector< std::vector<float> > pointsV);
  float angleVectors(float a1,float a2,float a3,float b1,float b2,float b3);
};

float distance(float *a, float *b);

float skeletonDistance(pi_tracker::Skeleton::ConstPtr msg, int a, int b);

#endif